// Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use std::{collections::HashMap, sync::Arc, time::Duration};

use lazy_static::lazy_static;
use log::{error, trace};
use ohmw_core::bincode::{BorrowDecode, Decode};
use ohmw_derive::BorrowDecode;
use rosrust::{Publisher, RawMessage, RawMessageDescription};
use tokio::{sync::Mutex, task::JoinHandle};

use crate::system::SystemHandles;

lazy_static! {
    static ref PUBLISHER_MAP: Arc<Mutex<HashMap<String, JoinHandle<()>>>> = Arc::new(Mutex::new(HashMap::new()));
}

#[derive(BorrowDecode)]
pub struct PublishTopicReq<'a> {
    pub topic_name: &'a str,
    pub info: Option<PublishTopicInfo<'a>>,
}

pub struct PublishTopicInfo<'a> {
    pub message_details: &'a [u8],
    pub hz: f32,
}

impl<'de> BorrowDecode<'de> for PublishTopicInfo<'de> {
    unsafe fn borrow_decode_unchecked<D: ohmw_core::bincode::Decoder>(decoder: &mut D, buf: &'de [u8]) -> Self {
        let message_details = unsafe { decoder.get_bytes_unchecked(buf, decoder.remaining(buf) - size_of::<f32>()) };
        let hz = unsafe { f32::decode_unchecked(decoder, buf) };
        PublishTopicInfo {
            message_details,
            hz,
        }
    }

    fn borrow_decode<D: ohmw_core::bincode::Decoder>(decoder: &mut D, buf: &'de [u8]) -> Result<Self, D::Error> {
        let message_details = decoder.get_bytes(buf, decoder.remaining(buf) - size_of::<f32>())?;
        let hz = f32::decode(decoder, buf)?;
        Ok(PublishTopicInfo {
            message_details,
            hz,
        })
    }
}

async fn sleep_by_hz(hz: f64) {
    if hz.is_sign_negative() {
        return;
    }

    let duration = Duration::from_secs_f64(1.0 / hz);
    tokio::time::sleep(duration).await;
}

async fn publisher_thread(
    publisher: Publisher<RawMessage>,
    topic_name: String,
    message_details: RawMessage,
    hz: f32,
) {
    trace!("Start publishing topic {}, hz={}, msg={:?}", topic_name, hz, message_details);
    if hz.is_sign_negative() {
        if let Err(err) = publisher.send(message_details) {
            error!("Failed to send message for topic {}: {}", topic_name, err);
            return;
        }
    } else {
        loop {
            if let Err(err) = publisher.send(message_details.clone()) {
                error!("Failed to send message for topic {}: {}", topic_name, err);
                return;
            }

            sleep_by_hz(hz as f64).await;
        }
    }
}

pub async fn publish_topic(req: PublishTopicReq<'_>) {
    if !SystemHandles::ensure_ros_initialized() {
        return;
    }
    
    if let Some(info) = req.info {
        let Some(msg_type) = SystemHandles::query_topic_msg_type(req.topic_name) else {
            error!("Failed to query topic msg type for topic {} to publish", req.topic_name);
            return;
        };

        let publisher = match rosrust::publish_with_description::<RawMessage>(
            req.topic_name,
            10,
            RawMessageDescription {
                msg_definition: "*".to_string(),
                md5sum: "*".to_string(),
                msg_type,
            },
        ) {
            Ok(publisher) => publisher,
            Err(e) => {
                error!("Failed to create publisher for topic {}: {}", req.topic_name, e);
                return;
            }
        };

        let topic_name = req.topic_name.to_string();
        let message_details = RawMessage(info.message_details.to_owned());
        let hz = info.hz;
        let thread = tokio::spawn(async move {
            publisher_thread(publisher, topic_name, message_details, hz).await
        });

        match PUBLISHER_MAP.lock().await.insert(req.topic_name.to_string(), thread) {
            Some(thread) => {
                thread.abort();
                trace!("Re-publish topic {}", req.topic_name)
            },
            None => trace!("Publish topic {}", req.topic_name),
        }
    } else {
        match PUBLISHER_MAP.lock().await.remove(req.topic_name) {
            Some(thread) => {
                thread.abort();
                trace!("Unpublish topic {}", req.topic_name)
            },
            None => error!("Topic {} is not published", req.topic_name),
        }
    }
}